#include "mqueue.h"


typedef enum {
	MQUEUE_LOCK_OFF = 0,
	MQUEUE_LOCK_ON  = 1,
} MQueueLock_e;

typedef enum {
	MQUEUE_FULL_FALSE = 0,
	MQUEUE_FULL_TRUE  = 1,
} MQueueFull_e;

void mQueueInit(MQueue_t *queue, void *buf, uint32_t len,
                MQueueOverWrite_e overwrite)
{
	queue->bufStart = buf;
	queue->bufEnd = ((uint8_t*)buf) + len;
	queue->len = len;
	queue->posRead = buf;
	queue->posWrite = buf;
	queue->overwrite = overwrite;
	queue->lock = MQUEUE_LOCK_OFF;
	queue->full = MQUEUE_FULL_FALSE;
}

static void mQueueWritePos(MQueue_t *queue)
{
	if (queue->posWrite == queue->bufEnd) {
		queue->posWrite = queue->bufStart;
	}
}

static void mQueueReadPos(MQueue_t *queue)
{
	if (queue->posRead == queue->bufEnd) {
		queue->posRead = queue->bufStart;
	}
}

static int mQueueWritePosLen(MQueue_t *queue)
{
	return queue->bufEnd - queue->posWrite;
}

static int mQueueReadPosLen(MQueue_t *queue)
{
	return queue->bufEnd - queue->posRead;
}

uint32_t mQueueDataLen(const MQueue_t *queue)
{
	if (queue->full) {
		return queue->len;
	}

	if (queue->posWrite < queue->posRead) {
		return queue->len - (queue->posRead - queue->posWrite);
	} else {
		return queue->posWrite - queue->posRead;
	}
}

uint32_t mQueueSpareLen(const MQueue_t *queue)
{
	if (queue->full) {
		return 0;
	}

	if (queue->posWrite < queue->posRead) {
		return queue->posRead - queue->posWrite;
	} else {
		return queue->len - (queue->posWrite - queue->posRead);
	}
}

MQueueRet_e mQueueWriteByte(MQueue_t *queue, uint8_t c)
{
	if (mQueueSpareLen(queue)) {
		mQueueWritePos(queue);
		*queue->posWrite++ = c;

		if (queue->posWrite == queue->posRead) {
			queue->full = MQUEUE_FULL_TRUE;
		}

		return MQUEUE_RET_OK;
	}

	return MQUEUE_RET_FULL;
}

uint32_t mQueueWriteBytes(MQueue_t *queue, const void *pBuf, uint32_t size)
{
	if (size == 0) {
		return 0;
	}

	uint32_t i;
	uint32_t endLen;
	uint32_t spareLen;
	const uint8_t *p = pBuf;

	i = 0;
	spareLen = mQueueSpareLen(queue);
	spareLen = spareLen > size ? size : spareLen;
	endLen = mQueueWritePosLen(queue);

	if (endLen < spareLen) {
		for (; i < endLen; ++i){
			*queue->posWrite++ = p[i];
		}

		queue->posWrite = queue->bufStart;
	}

	for (; i < spareLen; ++i) {
		*queue->posWrite++ = p[i];
	}

	if (queue->posWrite == queue->posRead) {
		queue->full = MQUEUE_FULL_TRUE;
	}

	return spareLen;
}

uint32_t mQueueWriteBytesFromReg(MQueue_t *queue, volatile const uint32_t *reg,
                                 uint32_t count)
{
	if (count == 0) {
		return 0;
	}

	uint32_t i;
	uint32_t endLen;
	uint32_t spareLen;

	i = 0;
	spareLen = mQueueSpareLen(queue);
	spareLen = spareLen > count ? count : spareLen;
	endLen = mQueueWritePosLen(queue);

	if (endLen < spareLen) {
		for (; i < endLen; ++i) {
			*queue->posWrite++ = *reg;
		}

		queue->posWrite = queue->bufStart;
	}

	for (; i < spareLen; ++i) {
		*queue->posWrite++ = *reg;
	}

	if (queue->posWrite == queue->posRead) {
		queue->full = MQUEUE_FULL_TRUE;
	}

	return spareLen;
}

uint32_t mQueueReadByte(MQueue_t *queue)
{
	mQueueReadPos(queue);

	if (mQueueDataLen(queue)) {
		queue->full = MQUEUE_FULL_FALSE;

		return *queue->posRead++;
	} else {
		return MQUEUE_RET_EMPTY;
	}
}

uint32_t mQueueReadBytes(MQueue_t *queue, void *pBuf, uint32_t size)
{
	if (size == 0) {
		return 0;
	}

	uint32_t i;
	uint32_t dataLen;
	uint32_t endLen;
	uint8_t *p = pBuf;

	i = 0;
	mQueueReadPos(queue);
	dataLen = mQueueDataLen(queue);
	dataLen = dataLen > size ? size : dataLen;
	endLen = mQueueReadPosLen(queue);

	if (endLen < dataLen) {
		for (; i < endLen; ++i) {
			p[i] = *queue->posRead++;
		}

		queue->posRead = queue->bufStart;
	}

	for (; i < dataLen; ++i) {
		p[i] = *queue->posRead++;
	}

	queue->full = MQUEUE_FULL_FALSE;

	return dataLen;
}

uint32_t mQueueReadBytesToReg(MQueue_t *queue, volatile uint32_t *reg,
                              uint32_t count)
{
	if (count == 0) {
		return 0;
	}

	uint32_t i;
	uint32_t dataLen;
	uint32_t endLen;

	i = 0;
	mQueueReadPos(queue);
	dataLen = mQueueDataLen(queue);
	dataLen = dataLen > count ? count : dataLen;
	endLen = mQueueReadPosLen(queue);

	if (endLen < dataLen) {
		for (; i < endLen; ++i) {
			*reg = *queue->posRead++;
		}

		queue->posRead = queue->bufStart;
	}

	for (; i < dataLen; ++i) {
		*reg = *queue->posRead++;
	}

	queue->full = MQUEUE_FULL_FALSE;

	return dataLen;
}

void mQueueDataClean(MQueue_t *queue)
{
	queue->posRead = queue->bufStart;
	queue->posWrite = queue->bufStart;
	queue->full = MQUEUE_FULL_FALSE;
}
